#include "defines.h"
#include "kernel.h"
#include "kozos.h"
#include "intr.h"
#include "interrupt.h"
#include "syscall.h"
#include "lib.h"
#include "memory.h"
#include "ipc.h"

#define THREAD_NUM 16
#define PRIORITY_NUM 16

static struct {
    kz_thread *head;
    kz_thread *tail;
} readyque[PRIORITY_NUM];

static kz_thread *current;
static kz_thread threads[THREAD_NUM];
static kz_handler_t handlers[SOFTVEC_TYPE_NUM];

void dispatch(kz_context *context);

int putready(kz_thread *p)
{
    if (!p)
        kz_sysdown("putready: pointer is NULL");
    if (p->flags & THREAD_READY)
        kz_sysdown("putready: already in readyque");

    if (readyque[p->priority].tail) {
        readyque[p->priority].tail->next = p;
    } else {
        readyque[p->priority].head = p;
    }
    readyque[p->priority].tail = p;
    p->flags |= THREAD_READY;

    return 0;
}

static int putcurrent()
{
    if (!current)
        return -1;

    putready(current);
    current = NULL;
    return 0;
}

static void thread_end(void)
{
    kz_exit();
}

static void thread_init(kz_thread *thp)
{
    thp->init.func(thp->init.argc, thp->init.argv);
    thread_end();
}

#define STACK_FROM_POOL 1   // スレッドスタックをメモリプールから確保(再利用可能)
static kz_thread *make_thread(kz_func_t func, char *name, int priority, unsigned stacksize, int argc, char *argv[])
{
    int i;
    kz_thread *thp;
    uint32 *sp;
#if ! STACK_FROM_POOL
    extern char userstack, euserstack;
    static char *thread_stack = &userstack;
#endif

    for (i = 0; i < THREAD_NUM; i++) {
        thp = &threads[i];
        if (!thp->init.func)
            break;
    }
    if (i == THREAD_NUM)
        return NULL;

    memset(thp, 0, sizeof(*thp));

    if (strlen(name) > THREAD_NAME_SIZE)
        kz_sysdown("make_thread: name is too long");
    strcpy(thp->name, name);
    thp->next = NULL;
    thp->priority = priority;
    thp->flags = 0;
    thp->init.func = func;
    thp->init.argc = argc;
    thp->init.argv = argv;

#if STACK_FROM_POOL
    thp->stack = (char *)kzmem_alloc(stacksize) + stacksize;
#else
    if (thread_stack + stacksize > &euserstack)
        kz_sysdown("make_thread: cannot allocate userstack");
    thread_stack += stacksize;
    thp->stack = thread_stack;
#endif
    memset(thp->stack - stacksize, 0, stacksize);
    thp->stacksize = stacksize;

    sp = (uint32 *)thp->stack;
    *--sp = (uint32)thread_end; /* thread_initから抜けたら飛ぶ(実際は来ない) */
    *--sp = (uint32)thread_init | (uint32)(priority ? 0 : 0xc0) << 24;
    /* ER6-ER0 */
    *--sp = 0;
    *--sp = 0;
    *--sp = 0;
    *--sp = 0;
    *--sp = 0;
    *--sp = 0;
    *--sp = (uint32)thp;

    thp->context.sp = (uint32)sp;

    return thp;
}

static kz_thread_id_t sys_run(kz_func_t func, char *name, int priority, unsigned stacksize, int argc, char *argv[])
{
    kz_thread *thp;
    thp = make_thread(func, name, priority, stacksize, argc, argv);
    if (!thp)
        kz_sysdown("make_thread: cannot make more threads");
    putready(thp);
    return (kz_thread_id_t)thp;
}

static int sys_exit(void)
{
    puts(current->name);
    puts(" EXIT.\n");
#if STACK_FROM_POOL
    kzmem_free(current->stack - current->stacksize);
#endif
    memset(current, 0, sizeof(*current));
    current = NULL;
    return 0;
}

static int sys_wait(void)
{
    return 0;
}

static int sys_sleep(void)
{
    current = NULL;
    return 0;
}

int sys_wakeup(kz_thread_id_t id)
{
    kz_thread *thp = (kz_thread *)id;
    
    if (thp->flags & THREAD_WAITMSG)
        recvmsg(thp->syscall.param->un.recv.id, thp); // メッセージは空のはず(kz_recvは失敗)
    
    if (!(thp->flags & THREAD_READY))
        putready(thp);

    return 0;
}

static kz_thread_id_t sys_getid(void)
{
    return (kz_thread_id_t)current;
}

static int sys_chpri(int priority)
{
    int old = current->priority;
    if (priority >= 0)
        current->priority = priority;
    return old;
}

static void *sys_kmalloc(int size)
{
    return kzmem_alloc(size);
}

static int sys_kmfree(void *p)
{
    kzmem_free(p);
    return 0;
}

static int sys_send(kz_msgbox_id_t id, int size, char *p)
{
    kz_thread *receiver = sendmsg(id, current, size, p);

    if (receiver) {
        sys_wakeup((kz_thread_id_t)receiver);
    }

    return size;
}

static kz_thread_id_t sys_recv(kz_msgbox_id_t id, int *sizep, char **pp, uint16 flags)
{
    if (msgbox_arrived(id, current)) {
        return recvmsg(id, current); // current->syscall.paramを通して書込む
    } else {
        if (!(flags & IPC_NOWAIT)) { /* 届くまで待ちに入る */
            msgbox_setreceiver(id, current);
            current = NULL;
        }
        return -1;
    }
}

static void thread_intr(softvec_type_t type, unsigned long sp);

static int sys_setintr(softvec_type_t type, kz_handler_t handler)
{
    softvec_setintr(type, thread_intr);

    handlers[type] = handler;

    return 0;
}

static kz_thread_info_t *sys_getthreads(int *sizep)
{
    int i, c;
    kz_thread_info_t *p, *ret;

    c = 0;
    for (i = 0; i < THREAD_NUM; i++) {
        if (threads[i].init.func) {
            ++c;
        }
    }

    *sizep = c;
    ret = kzmem_alloc(c * sizeof(kz_thread_info_t)); /* 解放は呼び出し元で */

    p = ret;
    for (i = 0; i < THREAD_NUM; i++) {
        if (threads[i].init.func) {
            strcpy(p->name, threads[i].name);
            p->priority = threads[i].priority;
            p->stacksize = threads[i].stacksize;
            p->flags = threads[i].flags;
            p++;
        }
    }

    return ret;
}

static kz_msgbox_id_t sys_getmsgbox()
{
    return msgbox_get();
}

static int sys_freemsgbox(kz_msgbox_id_t id)
{
    msgbox_free(id);
    return 0;
}

static int sys_resource()
{
    void resource_memory(), resource_msgbox();
    int c, i;

    c = 0;
    for (i = 0; i < THREAD_NUM; i++) {
        if (threads[i].init.func) {
            ++c;
        }
    }

    puts("THREAD: ");
    putval(c, 0);
    puts(" / ");
    putval(THREAD_NUM, 0);
    puts("\n");

    resource_memory();
    resource_msgbox();

    return 0;
}

static void call_run(kz_syscall_param_t *p)
{
    p->un.run.ret = sys_run(p->un.run.func, p->un.run.name, p->un.run.priority,
                                p->un.run.stacksize, p->un.run.argc, p->un.run.argv);
}

static void call_exit(kz_syscall_param_t *p)
{
    sys_exit();
}

static void call_wait(kz_syscall_param_t *p)
{
    p->un.wait.ret = sys_wait();
}

static void call_sleep(kz_syscall_param_t *p)
{
    p->un.sleep.ret = sys_sleep();
}

static void call_wakeup(kz_syscall_param_t *p)
{
    p->un.wakeup.ret = sys_wakeup(p->un.wakeup.id);
}

static void call_getid(kz_syscall_param_t *p)
{
    p->un.getid.ret = sys_getid();
}

static void call_chpri(kz_syscall_param_t *p)
{
    p->un.chpri.ret = sys_chpri(p->un.chpri.priority);
}

static void call_kmalloc(kz_syscall_param_t *p)
{
    p->un.kmalloc.ret = sys_kmalloc(p->un.kmalloc.size);
}

static void call_kmfree(kz_syscall_param_t *p)
{
    p->un.kmfree.ret = sys_kmfree(p->un.kmfree.p);
}

static void call_send(kz_syscall_param_t *p)
{
    p->un.send.ret = sys_send(p->un.send.id, p->un.send.size, p->un.send.p);
}

static void call_recv(kz_syscall_param_t *p)
{
    p->un.recv.ret = sys_recv(p->un.recv.id, p->un.recv.sizep, p->un.recv.pp, p->un.recv.flags);
}

static void call_setintr(kz_syscall_param_t *p)
{
    p->un.setintr.ret = sys_setintr(p->un.setintr.type, p->un.setintr.handler);
}

static void call_getthreads(kz_syscall_param_t *p)
{
    p->un.getthreads.ret = sys_getthreads(p->un.getthreads.sizep);
}

static void call_getmsgbox(kz_syscall_param_t *p)
{
    p->un.getmsgbox.ret = sys_getmsgbox();
}

static void call_freemsgbox(kz_syscall_param_t *p)
{
    p->un.freemsgbox.ret = sys_freemsgbox(p->un.freemsgbox.id);
}

static void call_resource(kz_syscall_param_t *p)
{
    p->un.resource.ret = sys_resource();
}

static void (*call_functions[])(kz_syscall_param_t *) = {
    call_run,
    call_exit,
    call_wait,
    call_sleep,
    call_wakeup,
    call_getid,
    call_chpri,
    call_kmalloc,
    call_kmfree,
    call_send,
    call_recv,
    call_setintr,
    call_getthreads,
    call_getmsgbox,
    call_freemsgbox,
    call_resource,
};

static void schedule(void)
{
    int i;
    for (i = 0; i < PRIORITY_NUM; i++) {
        if (readyque[i].head)
            break;
    }
    if (i == PRIORITY_NUM)
        kz_sysdown("schedule: no threads");

    current = readyque[i].head;
    // レディキューから外す。
    readyque[i].head = current->next;
    if (!readyque[i].head)
        readyque[i].tail = NULL;
    current->flags &= ~THREAD_READY;
    current->next = NULL;
}

static void syscall_intr(void)
{
    call_functions[current->syscall.type](current->syscall.param);
}

static void softerr_intr(void)
{
    puts(current->name);
    puts(" DOWN.\n");
    sys_exit();
}

/* OSの割込み処理の入口 */
static void thread_intr(softvec_type_t type, unsigned long sp)
{
    current->context.sp = sp;

    if (handlers[type])
        handlers[type]();

    putcurrent(); /* 戻す必要がない場合current = NULLになっている */

    schedule();

    dispatch(&current->context);
}

void kz_start(kz_func_t func, char *name, int priority, unsigned stacksize, int argc, char *argv[])
{
    memset(readyque, 0, sizeof(readyque));
    memset(threads, 0, sizeof(threads));
    memset(handlers, 0, sizeof(handlers));

    sys_setintr(SOFTVEC_TYPE_SYSCALL, syscall_intr);
    sys_setintr(SOFTVEC_TYPE_SOFTERR, softerr_intr);

    kzmem_init();
    ipc_init();

    current = make_thread(func, name, priority, stacksize, argc, argv); /* レディキューにあってはいけない */

    dispatch(&current->context);
}

void kz_sysdown(char *message)
{
    INTR_DISABLE;
    puts("SYSTEM ERROR! (");
    if (message) {
        puts(message);
    }
    puts(")\n");
    while (1)
        ;
}

void kz_syscall(kz_syscall_type_t type, kz_syscall_param_t *param)
{
    current->syscall.type = type;
    current->syscall.param = param;
    asm volatile ("trapa #0");
}

void kz_reset() // 内部リセット。WDT使用
{
    INTR_DISABLE;
    *((volatile uint16 *)0xffff8c) = 0xa567;
    while (1)
        ;
}

// サービスコール。システムコールと同等の機能を非スレッド(割り込みハンドラ)から呼ぶAPI。

int kx_wakeup(kz_thread_id_t id)
{
    return sys_wakeup(id);
}

void *kx_kmalloc(int size)
{
    return sys_kmalloc(size);
}

int kx_kmfree(void *p)
{
    return sys_kmfree(p);
}

int kx_send(kz_msgbox_id_t id, int size, char *p)
{
    kz_thread *tp;
    int ret;

    tp = current;
    current = NULL; /* 送り主スレッドはない */
    ret = sys_send(id, size, p);
    current = tp;

    return ret;
}
